
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms.h
  * @author     baiyang
  * @date       2021-8-12
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "mode.h"
#include <ahrs/ahrs_view.h>
#include <common/grapilot.h>
#include <arming/gp_arming.h>
#include <mavproxy/mavproxy.h>
#include <common/time/gp_time.h>
#include <common/filter/lpfilter.h>
#include <rc_channel/rc_channel.h>
#include <common/gp_config.h>
#include <common/location/location.h>
#include <common/gp_math/gp_mathlib.h>
#include <mc_attitude_control/mc_attitude_control_multi.h>
#include <mc_position_control/mc_position_control.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
// Documentation of GLobals:
typedef union {
    struct {
        uint8_t unused1                 : 1; // 0
        uint8_t unused_was_simple_mode  : 2; // 1,2
        uint8_t pre_arm_rc_check        : 1; // 3       // true if rc input pre-arm checks have been completed successfully
        uint8_t pre_arm_check           : 1; // 4       // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
        uint8_t auto_armed              : 1; // 5       // stops auto missions from beginning until throttle is raised
        uint8_t logging_started         : 1; // 6       // true if logging has started
        uint8_t land_complete           : 1; // 7       // true if we have detected a landing
        uint8_t new_radio_frame         : 1; // 8       // Set true if we have new PWM data to act on from the Radio
        uint8_t usb_connected_unused    : 1; // 9       // UNUSED
        uint8_t rc_receiver_present     : 1; // 10      // true if we have an rc receiver present (i.e. if we've ever received an update
        uint8_t compass_mot             : 1; // 11      // true if we are currently performing compassmot calibration
        uint8_t motor_test              : 1; // 12      // true if we are currently performing the motors test
        uint8_t initialised             : 1; // 13      // true once the init_ardupilot function has completed.  Extended status to GCS is not sent until this completes
        uint8_t land_complete_maybe     : 1; // 14      // true if we may have landed (less strict version of land_complete)
        uint8_t throttle_zero           : 1; // 15      // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock
        uint8_t system_time_set_unused  : 1; // 16      // true if the system time has been set from the GPS
        uint8_t gps_glitching           : 1; // 17      // true if GPS glitching is affecting navigation accuracy
        uint8_t using_interlock         : 1; // 20      // aux switch motor interlock function is in use
        uint8_t land_repo_active        : 1; // 21      // true if the pilot is overriding the landing position
        uint8_t motor_interlock_switch  : 1; // 22      // true if pilot is requesting motor interlock enable
        uint8_t in_arming_delay         : 1; // 23      // true while we are armed but waiting to spin motors
        uint8_t initialised_params      : 1; // 24      // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done
        uint8_t unused3                 : 1; // 25      // was compass_init_location; true when the compass's initial location has been set
        uint8_t unused2                 : 1; // 26      // aux switch rc_override is allowed
        uint8_t armed_with_switch       : 1; // 27      // we armed using a arming switch
    };
    uint32_t value;
} ap_t;

typedef struct {
    bool enabled:1;
    bool alt_healthy:1; // true if we can trust the altitude from the rangefinder
    int16_t alt_cm;     // tilt compensated altitude (in cm) from rangefinder
    float inertial_alt_cm; // inertial alt at time of last rangefinder sample
    uint32_t last_healthy_ms;
    LowPassFilt alt_cm_filt; // altitude filter
    int16_t alt_cm_glitch_protected;    // last glitch protected altitude
    int8_t glitch_count;    // non-zero number indicates rangefinder is glitching
    uint32_t glitch_cleared_ms; // system time glitch cleared
} RangeFinderState;

typedef struct
{
    ahrs_view *ahrs;

    // IMU variables
    // Integration time; time last loop took to run
    float G_Dt;

    // flight modes convenience array
    Param_int8 flight_modes[6];

    Mode *flightmode;
    ModeNumber prev_control_mode;

    ModeReason _last_reason;
    ModeReason control_mode_reason; // default, MODE_REASON_UNKNOWN

    // Failsafe
    struct {
        uint32_t terrain_first_failure_ms;  // the first time terrain data access failed - used to calculate the duration of the failure
        uint32_t terrain_last_failure_ms;   // the most recent time terrain data access failed

        int8_t radio_counter;            // number of iterations with throttle below throttle_fs_value

        uint8_t radio               : 1; // A status flag for the radio failsafe
        uint8_t gcs                 : 1; // A status flag for the ground station failsafe
        uint8_t ekf                 : 1; // true if ekf failsafe has occurred
        uint8_t terrain             : 1; // true if the missing terrain data failsafe has occurred
        uint8_t adsb                : 1; // true if an adsb related failsafe has occurred
    } failsafe;

    struct {
        // Throttle
        //
        Param_int8         failsafe_throttle;
        Param_int16        failsafe_throttle_value;
        Param_int16        throttle_deadzone;
    } g;

    // used to detect MAVLink acks from GCS to stop compassmot
    uint8_t command_ack_counter;

    bool standby_active;

    // 
    RCs_HandleTypeDef channels;

    // primary input control channels
    RC_HandleTypeDef *channel_roll;
    RC_HandleTypeDef *channel_pitch;
    RC_HandleTypeDef *channel_throttle;
    RC_HandleTypeDef *channel_yaw;

    // last valid RC input time
    uint32_t last_radio_update_ms;

    // filtered pilot's throttle input used to cancel landing if throttle held high
    LowPassFilt rc_throttle_control_in_filter;

    RangeFinderState rangefinder_state, rangefinder_up_state;

    Attitude_ctrl *attitude_control;
    Position_ctrl *pos_control;
    Motors_HandleTypeDef *motors;

    ap_t ap;

    Mode *mode_stabilize;
    Mode *mode_althold;

    // Altitude
    int32_t baro_alt;            // barometer altitude in cm above home
    LowPassFilt_vec3f land_accel_ef_filter; // accelerations for land and crash detector tests

    AirMode air_mode; // air mode is 0 = not-configured ; 1 = disabled; 2 = enabled

    // Stores initial bearing when armed - initial simple bearing is modified in super simple mode so not suitable
    int32_t initial_armed_bearing;

    // System Timers
    // --------------
    // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed.
    uint32_t arm_time_ms;

    // Used to exit the roll and pitch auto trim function
    uint8_t auto_trim_counter;
    bool auto_trim_started;    // default, false;

    // Arming/Disarming management class
    gp_arming arming;
}Fms_handle;

/*----------------------------------variable----------------------------------*/
extern Fms_handle fms;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
gp_err task_fms_init(void);

///Radio
void fms_init_rc_in();
void fms_read_radio();
void fms_enable_motor_output();

//---------------------------------- Mode ----------------------------------//
// return the static controller object corresponding to supplied mode
Mode *fms_mode_from_mode_num(const ModeNumber mode);

// called when an attempt to change into a mode is unsuccessful:
void fms_mode_change_failed(const Mode *mode, const char *reason);

// set_mode - change flight mode and perform any necessary initialisation
bool fms_set_mode(ModeNumber mode, ModeReason reason);
bool fms_set_mode_rc(int8_t new_pos, int8_t reason);

// update_flight_mode - calls the appropriate attitude controllers based on flight mode
// called at 100hz or more
void fms_update_flight_mode();

// exit_mode - high level call to organise cleanup as a flight mode is exited
void fms_exit_mode(Mode *old_flightmode, Mode *new_flightmode);

// notify_flight_mode - sets notify object based on current flight mode.  Only used for OreoLED notify device
void fms_notify_flight_mode();
//--------------------------------------------------------------------------//

//---------------------------------- land_detector -------------------------//
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
// called at MAIN_LOOP_RATE
void fms_update_land_detector();

// set land_complete flag and disarm motors if disarm-on-land is configured
void fms_set_land_complete(bool b);

// set land complete maybe flag
void fms_set_land_complete_maybe(bool b);

// run land and crash detectors
// called at MAIN_LOOP_RATE,200Hz
void fms_update_land_and_crash_detectors();
//--------------------------------------------------------------------------//

// get_non_takeoff_throttle - a throttle somewhere between min and mid throttle which should not lead to a takeoff
float fms_get_non_takeoff_throttle();

float fms_throttle_hover();

// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
// returns desired angle in centi-degrees
void fms_get_pilot_desired_lean_angles(float *roll_out, float *pitch_out, float angle_max, float angle_limit);

// transform pilot's manual throttle input to make hover throttle mid stick
// used only for manual throttle modes
// thr_mid should be in the range 0 to 1
// returns throttle output 0 to 1
float fms_get_pilot_desired_throttle();

// transform pilot's yaw input into a desired yaw rate
// returns desired yaw rate in centi-degrees per second
float fms_get_pilot_desired_yaw_rate(int16_t stick_angle);

// get_pilot_desired_climb_rate - transform pilot's throttle input to climb rate in cm/s
// without any deadzone at the bottom
float fms_get_pilot_desired_climb_rate(float throttle_control);

// It will return the PILOT_SPEED_DN value if non zero, otherwise if zero it returns the PILOT_SPEED_UP value.
uint16_t fms_get_pilot_speed_dn();
// It will return the PILOT_SPEED_UP value.
static inline uint16_t fms_get_pilot_speed_up(){ return abs(PARAM_GET_INT16(VEHICLE, PILOT_SPEED_UP));}
// It will return the PILOT_ACCEL_Z value.
static inline int16_t fms_get_pilot_accel_z(){ return abs(PARAM_GET_INT16(VEHICLE, PILOT_ACCEL_Z));}

// sets motors throttle_low_comp value depending upon vehicle state
//  low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
//  has no effect when throttle is above hover throttle
void fms_update_throttle_mix();

///AHRS
bool fms_position_ok();
bool fms_ekf_alt_ok();
void fms_update_origin();
void fms_update_ahrs();

// update_auto_armed - update status of auto_armed flag
void fms_update_auto_armed();

// rotate vector from vehicle's perspective to North-East frame
void fms_rotate_body_frame_to_NE(float *x, float *y);

///Arming
void fms_arming_ctor(gp_arming * arming);

bool fms_arming_arm(gp_arming * arming, const ArmingMethod method, const bool do_arming_checks);

// arming.disarm - disarm motors
bool fms_arming_disarm(gp_arming * arming, const ArmingMethod method, bool do_disarm_checks);

// performs pre-arm checks. expects to be called at 1hz.
void fms_arming_update(gp_arming * arming);

///Motors
// check for pilot stick input to trigger lost vehicle alarm
void fms_lost_vehicle_check();

// arm_motors_check - checks for pilot input to arm or disarm the copter
// called at 10hz
void fms_arm_motors_check();

// update estimated throttle required to hover (if necessary)
//  called at 100hz
void fms_update_throttle_hover();

// auto_disarm_check - disarms the copter if it has been sitting on the ground in manual mode with throttle low for at least 15 seconds
void fms_auto_disarm_check();

void fms_motors_output();

///State
//
void fms_set_auto_armed(bool b);
void fms_actuator_armed_notify(bool b);


///Param
void fms_param_assign();
void fms_param_update();

/// Standby
// Run standby functions at approximately 100 Hz to limit maximum variable build up
void fms_standby_update();

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



